require"vector"

local gravity=10
local bounce=0.4
local friction=0.6
local step=0.01
local timeleft=0

physics={}
local physics=physics

function physics.update(objects, dt)
  timeleft=timeleft+dt
  while(timeleft>step) do
    physics.separate(objects)
    for i,object in ipairs(objects) do
      physics.push(object, {0,0,-gravity*step})
      physics.updateObject(object, step)
      physics.walls(object,3,0,20)
      physics.walls(object,1,-8.5,8.5)
      physics.walls(object,2,-8.5,8.5)
      --bounce(-self.x,-self.y,0,self.x,self.y,self.z,self.bounce,self.friction)
      if math.abs(object.angularVelocity[3])<0.1 then object.angularVelocity[3]=0 end
    end
    timeleft=timeleft-step
  end
end

function physics.effect(object, impulse, leverArm)
  return Vector(impulse)/object.mass, vector.cross(leverArm or Vector{0,0,0},impulse)/object.angularMass
end

function physics.push(object, impulse, leverArm)
  local dv, da=physics.effect(object, impulse, leverArm)
  object.velocity=object.velocity+dv
  object.angularVelocity=object.angularVelocity+da
end

function physics.updateObject(object, dt)
  --dampening of velocities
  object.velocity=object.velocity*math.pow(0.7,dt)
  object.angularVelocity=object.angularVelocity*math.pow(0.7,dt)
  object.position=object.position+object.velocity*dt
  local k,w=vector.unit(object.angularVelocity), vector.abs(object.angularVelocity)*dt
  if w~=0.0 then for vertex in pairs(object.allVertices) do 
    vertex[1],vertex[2],vertex[3]=unpack(vector.rotate(vertex,k,w)) 
  end end
end

--bounce off a wall
function physics.wall(object, vertex, normal)
  
  --frictionless bounce
  -- idea is that the vertex that hit the wall will bounce back with a speed proportional to the impact speed
  local impactVelocity=object.velocity+vector.cross(object.angularVelocity,vertex)
  local normalSpeed=vector.dot(normal, impactVelocity) --impact speed
  if normalSpeed>0 then return end
  local linearEffectBase, angularEffectBase=physics.effect(object, normal, vertex) --effect of unit impulse
  local vertexEffectBase = linearEffectBase+vector.cross(angularEffectBase,vertex) --effect on the point of impact of a unit impulse
  
  local effect=(1+bounce)*normalSpeed/vector.dot(vertexEffectBase,normal) 
  physics.push(object, -normal*effect,vertex)

  --friction
  -- idea is that friction wants to stop "skating" on the surface 
  local skateVelocity=impactVelocity-normal*normalSpeed
  if vector.abs(skateVelocity)<0.01 then return end
  local frictionVector=vector.unit(skateVelocity)
  local linearEffectBase, angularEffectBase=physics.effect(object, frictionVector, vertex) --effect of unit impulse
  local vertexEffectBase = linearEffectBase+vector.cross(angularEffectBase,vertex) --effect on the point of impact of a unit impulse
 
  --we need an effect that is at most as big as one to stop skating
  local stopEffect = -vector.abs(skateVelocity)/vector.dot(vertexEffectBase,frictionVector)
  local frictionEffect = friction*effect
  if frictionEffect<stopEffect then frictionEffect=stopEffect end

  physics.push(object, frictionVector*frictionEffect,vertex)
end  

function physics.walls(object, coordinate, min, max)
  local low, high, lindex, hindex=min, max
  for i,vertex in ipairs(object.vertices) do
    if (vertex+object.position)[coordinate]<low then lindex,low=i,(vertex+object.position)[coordinate] end
    if (vertex+object.position)[coordinate]>high then hindex,high=i,(vertex+object.position)[coordinate] end
  end
  local normal=Vector{0,0,0}
  if lindex then
    normal[coordinate]=1
    physics.wall(object, object.vertices[lindex], normal) 
    object.position[coordinate]=object.position[coordinate]+min-low
  end
  if hindex then
    normal[coordinate]=-1
    physics.wall(object, object.vertices[hindex], normal) 
    object.position[coordinate]=object.position[coordinate]+max-high
  end
end

function physics.separate(objects)
  --when two dice collide, they bounce off each other as balls
  for i,object in ipairs(objects) do
    for j=i+1,#objects do
      local other=objects[j]
      local diff=object.position-other.position
      local distance=vector.abs(diff)
      if distance<=object.radius/20 then break end
      local normal=vector.unit(diff)
      if distance<object.radius+other.radius and vector.dot(diff, object.velocity-other.velocity)<0 then  -- touch each other AND go towards each other
        local objectNormalVelocity=normal*vector.dot(normal, object.velocity)
        local otherNormalVelocity=normal*vector.dot(normal, other.velocity)
        local impulse=object.mass*other.mass*other.mass*objectNormalVelocity - object.mass*object.mass*other.mass*otherNormalVelocity
        impulse=impulse/(object.mass*object.mass+other.mass*other.mass)
        physics.push(object,-impulse, -normal*object.radius*2)
        physics.push(other,impulse, normal*other.radius*2)
      end
    end
  end
end
